import numpy as np
import matplotlib.pyplot as plt
import copy
import math
import sys
import pathlib
import cubic_spline_planner
from scipy.spatial import distance
sys.path.append(str(pathlib.Path(__file__).parent.parent))


# parameter
MAX_T = 100.0  # maximum time to the goal [s]
MIN_T = 5.0  # minimum time to the goal[s]
SIM_LOOP = 500

# Parameter
MAX_SPEED = 50.0 / 3.6  # maximum speed [m/s]
MAX_ACCEL = 2.0  # maximum acceleration [m/ss]
MAX_CURVATURE = 1.0  # maximum curvature [1/m]
MAX_ROAD_WIDTH = 7.0  # maximum road width [m]
D_ROAD_W = 1.0  # road width sampling length [m]
DT = 0.2  # time tick [s]
MAX_T = 5.0  # max prediction time [m]
MIN_T = 4.0  # min prediction time [m]
TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
N_S_SAMPLE = 1  # sampling number of target speed
ROBOT_RADIUS = 2.0  # robot radius [m]

# cost weights
K_J = 0.1
K_T = 0.1
K_D = 1.0
K_LAT = 1.0
K_LON = 1.0

show_animation = True

"""
QuinticPolynomial 类 - 用于计算五次多项式系数以及多项式的位置、速度、加速度和 jerk（斜变率）。
quintic_polynomials_planner 函数 - 用于计算机器人的路径，包括位置、速度、加速度和 jerk。
"""

class QuinticPolynomial:
    """
    五次多项式类
    xs: 起始点位置
    vxs: 起始点速度
    axs: 起始点加速度
    xe: 终点位置
    vxe: 终点速度
    axe: 终点加速度
    time: 总时间
    """

    def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
        # 输入变道起始点和终点，以及变道时间
        # f = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4 + a5 * t^5,轨迹由五次多项式表示
        # 通过求导得到五次多项式的系数
        # 并提供轨迹方程求导的函数
        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[time ** 3, time ** 4, time ** 5],
                      [3 * time ** 2, 4 * time ** 3, 5 * time ** 4],
                      [6 * time, 12 * time ** 2, 20 * time ** 3]])
        b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2,
                      vxe - self.a1 - 2 * self.a2 * time,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)   # 返回求解的多项式系数Ax=b

        self.a3 = x[0]
        self.a4 = x[1]
        self.a5 = x[2]

    def calc_point(self, t):
        # calculate the position after time t
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
            self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

        return xt

    def calc_first_derivative(self, t):
        # calculate the velocity after time t(即求一阶导)
        xt = self.a1 + 2 * self.a2 * t + \
            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4

        return xt

    def calc_second_derivative(self, t):
        # calculate the second derivative after time t(即求二阶导)
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * \
            self.a4 * t ** 2 + 20 * self.a5 * t ** 3

        return xt

    def calc_third_derivative(self, t):
        # calculate the third derivative after time t(即求三阶导)
        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

        return xt


def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
    """
    quintic polynomial planner
    五次规划器
    input
        s_x: start x position [m]
        s_y: start y position [m]
        s_yaw: start yaw angle [rad]
        sa: start accel [m/ss]
        gx: goal x position [m]
        gy: goal y position [m]
        gyaw: goal yaw angle [rad]
        ga: goal accel [m/ss]
        max_accel: maximum accel [m/ss]
        max_jerk: maximum jerk [m/sss]  # 加速度求导（加加速度）
        dt: time tick [s]
    return
        time: time result
        rx: x position result list
        ry: y position result list
        ryaw: yaw angle result list
        rv: velocity result list
        ra: accel result list
    """

    vxs = sv * math.cos(syaw)
    vys = sv * math.sin(syaw)
    vxg = gv * math.cos(gyaw)
    vyg = gv * math.sin(gyaw)

    axs = sa * math.cos(syaw)
    ays = sa * math.sin(syaw)
    axg = ga * math.cos(gyaw)
    ayg = ga * math.sin(gyaw)

    Time, Rx, Ry, Ryaw, Rv, Ra, Rj = [], [], [], [], [], [], []

    for T in np.arange(MIN_T, MAX_T, MIN_T):    # 对于整数，同range;对于实数，同linspace
        # 将x,y方向分开计算
        # 达goal的最短Time到最长Time内的每个T代表一种轨迹
        xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
        yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)

        time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []

        for t in np.arange(0.0, T + dt, dt):
            # 计算在变道过程中的x,y方向pos,vel,acc,jerk
            # 时间间隔dt
            time.append(t)
            rx.append(xqp.calc_point(t))
            ry.append(yqp.calc_point(t))

            vx = xqp.calc_first_derivative(t)
            vy = yqp.calc_first_derivative(t)
            v = np.hypot(vx, vy)
            yaw = math.atan2(vy, vx)
            rv.append(v)
            ryaw.append(yaw)

            ax = xqp.calc_second_derivative(t)
            ay = yqp.calc_second_derivative(t)
            a = np.hypot(ax, ay)
            if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
                a *= -1
            ra.append(a)

            # 计算加加速度
            jx = xqp.calc_third_derivative(t)
            jy = yqp.calc_third_derivative(t)
            j = np.hypot(jx, jy)
            if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
                j *= -1
            rj.append(j)

        if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
            # 最大加速度跟最大加加速度不违规
            print("find path!!")
            Time = time
            Rx = rx
            Ry = ry
            Ryaw = ryaw
            Rv = rv
            Ra = ra
            Rj = rj
            break

    # if show_animation:  # pragma: no cover
    #     # show animation:flag，用来显示图画
    #     # 可视化接口将使用一安
    #     for i, _ in enumerate(time):
    #         plt.cla()
    #         # for stopping simulation with the esc key.
    #         plt.gcf().canvas.mpl_connect('key_release_event',
    #                                      lambda event: [exit(0) if event.key == 'escape' else None])
    #         plt.grid(True)
    #         plt.axis("equal")
    #         plot_arrow(sx, sy, syaw)
    #         plot_arrow(gx, gy, gyaw)
    #         plot_arrow(rx[i], ry[i], ryaw[i])
    #         plt.title("Time[s]:" + str(time[i])[0:4] +
    #                   " v[m/s]:" + str(rv[i])[0:4] +
    #                   " a[m/ss]:" + str(ra[i])[0:4] +
    #                   " jerk[m/sss]:" + str(rj[i])[0:4],
    #                   )
    #         plt.pause(0.001)

    return Time, Rx, Ry, Ryaw, Rv, Ra, Rj


def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):  # pragma: no cover
    """
    Plot arrow
    
    """

    if not isinstance(x, float):
        for (ix, iy, iyaw) in zip(x, y, yaw):
            # 将x,y,yaw的list重新组合为tuple，tuple中每个tuple都是(x,y,yaw)
            # 如:((x1,y1,yaw1),(x2,y2,yaw2)...)
            plot_arrow(ix, iy, iyaw)
    else:
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
                  fc=fc, ec=ec, head_width=width, head_length=width)
        plt.plot(x, y)




show_animation = True


class QuarticPolynomial:
    """
    四次多项式类
    xs:起始点
    vxs:起始点速度
    axs:起始点加速度
    vxe:终止点速度
    axe:终止点加速度
    time:总耗时
    """
    def __init__(self, xs, vxs, axs, vxe, axe, time):
        # calc coefficient of quartic polynomial
        print("xs",xs)
        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[3 * time ** 2, 4 * time ** 3],
                      [6 * time, 12 * time ** 2]])
        b = np.array([vxe - self.a1 - 2 * self.a2 * time,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)

        self.a3 = x[0]
        self.a4 = x[1]

    def calc_point(self, t):
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
            self.a3 * t ** 3 + self.a4 * t ** 4
        print("a0",self.a0)
        print("a1",self.a1)
        print("a2",self.a2)
        print("a3",self.a3)
        print("a4",self.a4)
        return xt

    def calc_first_derivative(self, t):
        xt = self.a1 + 2 * self.a2 * t + \
            3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3

        return xt

    def calc_second_derivative(self, t):
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2

        return xt

    def calc_third_derivative(self, t):
        xt = 6 * self.a3 + 24 * self.a4 * t

        return xt


class FrenetPath:
    """
    Frenet坐标系下的路径
    """
    def __init__(self):
        self.t = [] # t坐标
        self.d = [] # d坐标
        self.d_d = []
        self.d_dd = []
        self.d_ddd = []
        self.s = [] # s坐标
        self.s_d = []
        self.s_dd = []
        self.s_ddd = []
        self.cd = 0.0
        self.cv = 0.0
        self.cf = 0.0

        self.x = []
        self.y = []
        self.yaw = []
        self.ds = []
        self.c = []


def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0,Ti,di):
    """_summary_

    Args:
        c_speed (_type_): current speed [m/s]
        c_accel (_type_): current acceleration [m/ss]
        c_d (_type_): current lateral position [m]
        c_d_d (_type_): current lateral speed [m/s]
        c_d_dd (_type_): current lateral acceleration [m/ss]
        s0 (_type_): 车辆起始的s坐标
        di(int):frenet坐标的d方向（横向偏移量）

    Returns:
        _type_: _description_
    """
    # 找到一系列的frenet坐标系下的路径

    # generate path to each offset goal
    # frenet坐标的d方向（横向偏移量）
    # D_ROAD_W为sampling length

    # Lateral（横向） motion planning
    # 对总时长为Ti的路径进行规划
    fp = FrenetPath()   # 用FrenectPath类来存储路径信息

    # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
    lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)    # 对于每个di（横向终点）
    # di是对于每一点横向偏移

    # 计算参数的List
    fp.t = [t for t in np.arange(0.0, Ti, DT)]  # 对Ti，frenet坐标系下的时间
    fp.d = [lat_qp.calc_point(t) for t in fp.t]
    fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
    fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
    fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
    tv = c_speed  # target speed
    # Longitudinal（纵向） motion planning (Velocity keeping)

    # 纵向使用四次多项式进行规划
    lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti)

    fp.s = [lon_qp.calc_point(t) for t in fp.t]
    fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
    fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
    fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

    Jp = sum(np.power(fp.d_ddd, 2))  # square of d.jerk
    Js = sum(np.power(fp.s_ddd, 2))  # square of s.jerk

    # square of diff from target speed
    ds = (tv - fp.s_d[-1]) ** 2

    fp.cd = K_J * Jp + K_T * Ti + K_D * fp.d[-1] ** 2
    fp.cv = K_J * Js + K_T * Ti + K_D * ds
    fp.cf = K_LAT * fp.cd + K_LON * fp.cv
    return fp


def calc_global_paths(fp, csp,cx,cy):
    # fplist为frenet坐标系下的路径列表
    # csp为插值后的道路曲线
    # calc global positions
    deltaX = 0
    deltaY = 0
    for i in range(len(fp.s)):
        # print("len",len(fp.s))
        print("fp.s[i]:",fp.s[i])
        ix, iy = csp.calc_position(fp.s[i])
        if ix is None:
            break
        print("ix,iy",ix,iy)
        i_yaw = csp.calc_yaw(fp.s[i])
        print("i_yaw",i_yaw)
        di = fp.d[i]
        fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
        fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
        print("i_yaw:",i_yaw)
        print("di:",di)
        print("**************")
        # print("fx,fy",fx,fy)
        fp.x.append(fx)
        fp.y.append(fy)
    # deltaX = fp.x[0] - cx
    # deltaY = fp.y[0] - cy
    # for i in range(len(fp.x)):
    #     fp.x[i] = fp.x[i] - deltaX
    #     fp.y[i] = fp.y[i] - deltaY

    # calc yaw and ds
    for i in range(len(fp.x) - 1):
        dx = fp.x[i + 1] - fp.x[i]
        dy = fp.y[i + 1] - fp.y[i]
        fp.yaw.append(math.atan2(dy, dx))
        fp.ds.append(math.hypot(dx, dy))

    fp.yaw.append(fp.yaw[-1])
    fp.ds.append(fp.ds[-1])
    print("calc curvature")
    # calc curvature
    for i in range(len(fp.yaw) - 1):
        fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])

    return fp  # 通过frenet坐标系下的路径列表，计算出全局坐标系下的路径列表


def check_collision(fp, ob):
    # 检测是否会造成碰撞
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True


def check_paths(fp, ob):
    # 检测路径是否合法
    flag = True
    if any([v > MAX_SPEED for v in fp.s_d]):  # Max speed check
        flag = False
    elif any([abs(a) > MAX_ACCEL for a in
                fp.s_dd]):  # Max accel check
        flag = False
    elif any([abs(c) > MAX_CURVATURE for c in
                fp.c]):  # Max curvature check
        flag = False
    elif ((not ob==None) and (not check_collision(fp, ob))):
        flag = False

    return flag


def frenet_optimal_planning(cx,cy,csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob,Ti,di) -> FrenetPath:
    fp = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0,Ti,di)
    fplist = calc_global_paths(fp, csp,cx,cy)     # 最终要得到的是global坐标系下的路径
    # if not check_paths(fp, ob):
    #     return False
    # else:
    #     return fp
    return fp


def generate_target_course(x, y):
    """计算道路样条曲线，xy为道路点的坐标"""
    csp = cubic_spline_planner.CubicSpline2D(x, y)
    # csp是样条曲线的对象
    s = np.arange(0, csp.s[-1], 0.1)

    rx, ry, ryaw, rk = [], [], [], []
    for i_s in s:
        # print("i_s",i_s)
        ix, iy = csp.calc_position(i_s)
        # print("ix,iy",ix,iy)
        # 道路样条曲线上的点
        rx.append(ix)
        ry.append(iy)
        ryaw.append(csp.calc_yaw(i_s))
        rk.append(csp.calc_curvature(i_s))

    return rx, ry, ryaw, rk, csp


def closest_points(x, y, ref_x, ref_y):
    """
    Find the two closest points in the reference path to the car's position.
    """
    print("len(ref_x)",len(ref_x))

    distances = [distance.euclidean((x, y), (rx, ry)) for rx, ry in zip(ref_x, ref_y)]
    closest_idx = np.argmin(distances)
    
    if closest_idx == 0:
        next_idx = 1
    elif closest_idx == len(ref_x) - 1:
        next_idx = closest_idx - 1
    else:
        next_idx = closest_idx + 1 if (ref_x[closest_idx + 1] - ref_x[closest_idx]) * (x - ref_x[closest_idx]) + (ref_y[closest_idx + 1] - ref_y[closest_idx]) * (y - ref_y[closest_idx]) > 0 else closest_idx - 1
    
    return closest_idx, next_idx

def frenet_coordinates_improved(car, line):
    """
    Compute the Frenet coordinates (s, d) of the car given its Cartesian coordinates
    and a reference path. This version also considers the heading angle of the car.
    """
    car_x, car_y, car_theta = car[0], car[1], car[2]
    ref_x, ref_y = line[0], line[1]
    closest_idx, next_idx = closest_points(car_x, car_y, ref_x, ref_y)
    
    # Compute s
    s = sum(np.sqrt((ref_x[i+1] - ref_x[i])**2 + (ref_y[i+1] - ref_y[i])**2) for i in range(closest_idx))
    s += np.sqrt((car_x - ref_x[closest_idx])**2 + (car_y - ref_y[closest_idx])**2)
    
    # Compute d
    dx = ref_x[next_idx] - ref_x[closest_idx]
    dy = ref_y[next_idx] - ref_y[closest_idx]
    
    # Vector from the closest reference point to the car
    vec_to_car = [car_x - ref_x[closest_idx], car_y - ref_y[closest_idx]]
    
    # Cross product
    cross_product = vec_to_car[0] * dy - vec_to_car[1] * dx
    
    # Direction vector from closest_idx to next_idx
    direction_vector = [dx, dy]
    
    # Compute the angle between the direction vector and the car's heading
    angle = np.arctan2(direction_vector[1], direction_vector[0]) - car_theta
    
    # Make sure the angle is between -pi and pi
    angle = np.arctan2(np.sin(angle), np.cos(angle))
    
    d = np.sqrt(vec_to_car[0]**2 + vec_to_car[1]**2)
    
    # Determine the sign of d based on the cross product and the angle
    if np.abs(angle) < np.pi / 2:
        d *= np.sign(cross_product)
    else:
        d *= -np.sign(cross_product)
    
    return s, d


testPathX3 = [5259.666412270729, 5259.922642977613, 5260.178240499553, 5260.433206988063, 5260.687544643583, 5260.941255703583, 5261.210525337493, 5261.47908965853, 5261.746950200655, 5262.014108509731, 5262.280566138233, 5262.546324651859, 5262.811385620276, 5263.075750630341, 5263.339421272875, 5263.602399150607, 5263.864685875517, 5264.126283067522, 5264.387192358439, 5264.647415386699, 5264.9069538026315, 5265.165809261857, 5265.423983433222, 5265.681477993503, 5265.938294624769, 5266.194435023634, 5266.4499008933235, 5266.704693942355, 5266.95881589379, 5267.212268474658, 5267.465053423888, 5267.7171724883465, 5267.968627420187, 5268.219419983465, 5268.469551948848, 5268.719025097584, 5268.967841216208, 5269.216002101836, 5269.463509558196, 5269.710365396948, 5269.95657143901, 5270.202129513235, 5270.447041453766, 5270.691309106645, 5270.934934323205, 5271.1779189640365, 5271.420264893695, 5271.661973988639, 5271.90304813194, 5272.14620533551, 5272.388716660605, 5272.630583696726, 5272.8718080426315, 5273.112391303691, 5273.352335089242, 5273.591641015234, 5273.830310702905, 5274.068345776139, 5274.305747865431, 5274.5425186052435, 5274.778659632684, 5275.014172591472, 5275.249059126648, 5275.483320885898, 5275.716959520877, 5275.94997668456, 5276.1823740338905, 5276.414153227132, 5276.64531592123, 5276.875863779737, 5277.105798462242, 5277.335121633622, 5277.563834954788, 5277.791940090616, 5278.019438703338, 5278.246332455187, 5278.472623007074, 5278.698312022553, 5278.923401158568, 5279.147892073382, 5279.371786421295, 5279.59508585925, 5279.817792034933, 5280.039906597352, 5280.261431192871, 5280.482367459921, 5280.702717038253, 5280.922481561006, 5281.141662656032, 5281.36026194986, 5281.578281059759, 5281.79572160168, 5282.012585183638, 5282.228873407035, 5282.444587870629, 5282.659730161278, 5282.874301864516, 5283.088304555297, 5283.3017398019665, 5283.514609166255, 5283.726914199315, 5283.938656447009, 5284.149837444621, 5284.3604587168575, 5284.570521784454, 5284.780028153604, 5284.988979321241, 5285.197376777688, 5285.405221997401, 5285.405221997401, 5285.610674403723, 5285.815595317496, 5286.019993834053, 5286.223878607053, 5286.427257859058, 5286.630139385504, 5286.8325305718845, 5287.034438397726, 5287.235869443194, 5287.4368299076095, 5287.637325612091, 5287.837362008816, 5288.036944196881, 5288.236076922313, 5288.434764599218, 5288.633011308462, 5288.830820814863, 5289.028196576446, 5289.225141748411, 5289.421659200326, 5289.617751521409, 5289.813421035086, 5290.008669801624, 5290.20349963533, 5290.39791211116, 5290.591908576618, 5290.785490158373, 5290.978657776804, 5291.171412153928, 5291.363753822666, 5291.555683137415, 5291.747200289921, 5291.938305309276, 5292.1289980817555, 5292.31927835214, 5292.509145744873, 5292.698599764063, 5292.88763981067, 5293.0762651878, 5293.264475117894, 5293.45226874537, 5293.63964515382, 5293.82660336997, 5294.013142380879, 5294.199261140541, 5294.384958579152, 5294.384958579152, 5294.565621872022, 5294.746158433221, 5294.926686942445, 5295.107195459917, 5295.287584949909, 5295.473220527553, 5295.658569139342, 5295.843621641013, 5296.028368881691, 5296.212801702569, 5296.396910939548, 5296.580687420597, 5296.764121961782, 5296.947205377847, 5297.129928470313, 5297.312282034087, 5297.494256856143, 5297.6758437142, 5297.8570333767175, 5298.0378166029, 5298.21818414534, 5298.398126743406, 5298.57763512853, 5298.756700024214, 5298.935312142056, 5298.935312142056, 5299.108588625918, 5299.281421727451, 5299.453804439361, 5299.6257289926625, 5299.79718689106, 5299.968168947973, 5300.138665324892, 5300.308665565752, 5300.478158633964, 5300.647132950763, 5300.815576430914, 5300.983476517089, 5301.150820220865, 5301.317594158429, 5301.483784583633, 5301.649377432958, 5301.8143583532865, 5301.978712746858, 5302.142425801687, 5302.3054825365225, 5302.467867829942, 5302.629566466635, 5302.629566466635, 5302.791101147964, 5302.951921105036, 5303.1120182329805, 5303.271384418993, 5303.430011542332, 5303.587891467714, 5303.745016053241, 5303.90137714379, 5304.056966576307, 5304.211776174511, 5304.365797752867, 5304.5190231165825, 5304.67144405764, 5304.823052357444, 5304.973839788142, 5305.123798111303, 5305.272919075271, 5305.421194419135, 5305.56768664775, 5305.713338711081, 5305.8581535659705, 5306.002134187776, 5306.145283575656, 5306.2876047565405, 5306.429100791739, 5306.569774779591, 5306.709629860751, 5306.848669224799, 5306.986896114214, 5307.124313828334, 5307.260925731298, 5307.396735250714, 5307.531745890893, 5307.6659612301955, 5307.799384932938, 5307.932020746748, 5308.063872515786, 5308.194944178102, 5308.325239777535, 5308.454763462393, 5308.454763462393, 5308.58568745843, 5308.715823639586, 5308.845177885169, 5308.973755959432, 5309.101563507622, 5309.228606069189, 5309.354889075151, 5309.48041785338, 5309.60519763389, 5309.7292335541315, 5309.852530657664, 5309.975093904736, 5310.096928170967, 5310.218038255274, 5310.338428881202, 5310.458104702208, 5310.577070306954, 5310.695330220626, 5310.812888911547, 5310.929750792502, 5311.045920228669, 5311.161401537623, 5311.276198999907, 5311.390316853754, 5311.503759308299, 5311.616530540943, 5311.728634706607, 5311.840075940375, 5311.95085836014, 5312.060986070571, 5312.170463171049, 5312.279293756987, 5312.279293756987, 5312.390238886433, 5312.500517468594, 5312.610138637153, 5312.719111535051, 5312.827445311837, 5312.935149130289, 5313.0422321597935, 5313.148703578994, 5313.254572577114, 5313.359848349989, 5313.464540101387, 5313.568657045657, 5313.672208402439, 5313.775203401946, 5313.877651278363, 5313.979561275133, 5314.080942644951, 5314.181804643162, 5314.28215653436, 5314.382007591083, 5314.48136709115, 5314.580244316353, 5314.678648557737, 5314.776589111638, 5314.874075278361, 5314.971116367462, 5315.067721691149, 5315.163900566915, 5315.259662317542, 5315.35501627375, 5315.449971766256, 5315.544538136356, 5315.638724724025, 5315.732540877173, 5315.8259959476745, 5315.91909929005, 5316.011860266755, 5316.1042882376, 5316.196392572973, 5316.288182643265, 5316.379667822831, 5316.470857489995, 5316.561761024403, 5316.652387813635, 5316.742747242626, 5316.8328487016015, 5316.922701586076, 5317.012054637115, 5317.101146502109, 5317.189953807981, 5317.2784531750385, 5317.366621220949, 5317.454434555443, 5317.541869785608, 5317.628903517206, 5317.715512348069, 5317.801672876026, 5317.887361694939, 5317.9725553973485, 5318.05723057315, 5318.141363813562, 5318.224931707157, 5318.307910842509, 5318.390277810834, 5318.47200920203, 5318.553081609957, 5318.633471627158, 5318.713155855427, 5318.792110895241]

testPathY3 =  [9225.351339797398, 9225.734252453303, 9226.117588059195, 9226.501343678718, 9226.88551635661, 9227.27010313681, 9227.679765297711, 9228.089890195168, 9228.500475096149, 9228.91151727629, 9229.323014023052, 9229.734962634924, 9230.147360419856, 9230.560204696829, 9230.97349279507, 9231.387222054838, 9231.801389826636, 9232.215993472004, 9232.631030362722, 9233.046497881607, 9233.46239342014, 9233.878714381628, 9234.2954581812, 9234.712622241856, 9235.13020399843, 9235.548200895992, 9235.966610390647, 9236.385429947963, 9236.80465704374, 9237.224289164824, 9237.6443238083, 9238.064758482284, 9238.485590703565, 9238.90681800075, 9239.328437911907, 9239.750447986133, 9240.172845781195, 9240.595628867468, 9241.018794823209, 9241.442341238499, 9241.866265712868, 9242.2905658561, 9242.715239289006, 9243.140283641067, 9243.565696552798, 9243.991475674171, 9244.417618666977, 9244.844123200104, 9245.270986955044, 9245.703042286512, 9246.135460483834, 9246.568239387698, 9247.001376851395, 9247.434870732946, 9247.868718903772, 9248.30291924081, 9248.737469633603, 9249.17236797879, 9249.60761218483, 9250.04320016885, 9250.479129857433, 9250.91539918741, 9251.352006106645, 9251.788948571664, 9252.226224550825, 9252.663832021934, 9253.101768973043, 9253.540033403244, 9253.978623322651, 9254.41753675006, 9254.856771718438, 9255.296326268644, 9255.73619845414, 9256.176386338637, 9256.616887997661, 9257.057701516991, 9257.498824995007, 9257.940256540342, 9258.381994273443, 9258.824036326581, 9259.266380843063, 9259.709025978798, 9260.15196989916, 9260.59521078449, 9261.038746824588, 9261.482576221077, 9261.926697189761, 9262.371107955905, 9262.815806758172, 9263.260791847826, 9263.706061487175, 9264.151613951917, 9264.597447527996, 9265.043560516333, 9265.48995122809, 9265.936617988613, 9266.383559133497, 9266.8307730141, 9267.278257992017, 9267.726012440671, 9268.174034749245, 9268.622323316376, 9269.070876554892, 9269.519692891798, 9269.968770764352, 9270.418108624779, 9270.867704935556, 9271.317558175702, 9271.767666833699, 9271.767666833699, 9272.2140152607, 9272.660607949614, 9273.107439974994, 9273.55450665097, 9274.001803526513, 9274.449326376773, 9274.897071198347, 9275.345034197462, 9275.793211788394, 9276.24160058401, 9276.690197389471, 9277.138999196706, 9277.588003176543, 9278.037206673178, 9278.486607197883, 9278.936202424275, 9279.385990180423, 9279.835968441777, 9280.286135329574, 9280.736489100598, 9281.187028144033, 9281.637750975942, 9282.088656230595, 9282.539742659688, 9282.991009121306, 9283.442454580712, 9283.89407809931, 9284.345878833868, 9284.79785602626, 9285.25000900426, 9285.702337172084, 9286.154840004881, 9286.607517047933, 9287.06036790799, 9287.513392248547, 9287.9665897859, 9288.419960285988, 9288.873503554949, 9289.327219437531, 9289.781107814735, 9290.235168591995, 9290.68940170232, 9291.143807096856, 9291.598384740922, 9292.05313461167, 9292.508056691757, 9292.508056691757, 9292.951645213463, 9293.395285334926, 9293.838928733978, 9294.282580266714, 9294.72628020594, 9295.183598159123, 9295.641032493886, 9296.098586696658, 9296.556264239684, 9297.0140685755, 9297.472003137733, 9297.930071341887, 9298.388276581396, 9298.84662223157, 9299.305111645648, 9299.763748154814, 9300.22253506975, 9300.681475677504, 9301.140573241479, 9301.599831002224, 9302.059252176647, 9302.518839954071, 9302.978597501757, 9303.43852795859, 9303.898634437452, 9303.898634437452, 9304.346307904687, 9304.794152734086, 9305.242171117277, 9305.690365513829, 9306.138738636282, 9306.587293429662, 9307.036033053344, 9307.484960866088, 9307.934080405548, 9308.383395371717, 9308.832909611172, 9309.28262709579, 9309.732551908573, 9310.182688226296, 9310.633040299814, 9311.083612439881, 9311.534409000587, 9311.985434358883, 9312.436692901962, 9312.88818901071, 9313.339927041585, 9313.791911310856, 9313.791911310856, 9314.245656677549, 9314.699655852062, 9315.153910423936, 9315.60842194488, 9316.063191927205, 9316.518221846181, 9316.973513138462, 9317.429067198931, 9317.88488538544, 9318.340969013276, 9318.797319359115, 9319.253937657866, 9319.710825101092, 9320.167982841744, 9320.625411988634, 9321.083113608025, 9321.54108872362, 9321.999338314205, 9322.454957547385, 9322.910846057435, 9323.36700119959, 9323.823420344053, 9324.280100874432, 9324.737040185359, 9325.194235684081, 9325.651684785724, 9326.109384912508, 9326.567333495319, 9327.025527968199, 9327.4839657707, 9327.942644344746, 9328.401561133835, 9328.860713583035, 9329.320099135846, 9329.779715234978, 9330.239559320771, 9330.699628828845, 9331.159921191662, 9331.620433837745, 9332.081164186162, 9332.081164186162, 9332.549894441809, 9333.018844031883, 9333.488009910492, 9333.957389096358, 9334.426978673622, 9334.896775785523, 9335.366777636771, 9335.836981488032, 9336.307384657499, 9336.7779845138, 9337.24877847758, 9337.719764021494, 9338.19093866154, 9338.662299963367, 9339.133845533603, 9339.605573023793, 9340.077480123315, 9340.549564561734, 9341.021824106443, 9341.4942565603, 9341.966859758475, 9342.439631571597, 9342.912569898668, 9343.385672671002, 9343.858937846704, 9344.33236341068, 9344.805947373834, 9345.279687772294, 9345.753582663465, 9346.227630128391, 9346.701828267822, 9347.176175200631, 9347.176175200631, 9347.662794889193, 9348.149566063184, 9348.636485708234, 9349.123550845428, 9349.61075853369, 9350.098105870551, 9350.585589985067, 9351.073208046486, 9351.560957256366, 9352.048834850944, 9352.536838100337, 9353.024964309343, 9353.51321081507, 9354.00157498536, 9354.490054221938, 9354.978645957272, 9355.467347654558, 9355.956156806944, 9356.445070938307, 9356.93408760011, 9357.423204373765, 9357.912418869048, 9358.401728722534, 9358.891131598382, 9359.38062518754, 9359.870207206968, 9360.359875398051, 9360.849627528187, 9361.339461389984, 9361.829374798122, 9362.319365592499, 9362.809431634287, 9363.299570808304, 9363.789781020645, 9364.28006019868, 9364.770406291063, 9365.260817265356, 9365.751291108825, 9366.241825830803, 9366.732419454804, 9367.223070026415, 9367.713775605403, 9368.20453427045, 9368.695344117576, 9369.186203255409, 9369.677109811491, 9370.168061925977, 9370.657658592498, 9371.147302853009, 9371.636998801541, 9372.126750494295, 9372.616561939405, 9373.106437098502, 9373.596379882787, 9374.086394146714, 9374.576483685638, 9375.066652235013, 9375.556903462524, 9376.047240968868, 9376.537668280667, 9377.028188849672, 9377.518806046464, 9378.009523160452, 9378.500343394357, 9378.991269860273, 9379.48230557809, 9379.973453470766, 9380.464716359593, 9380.956096965783]
testPathX2 = [5259.666412270729, 5259.922642977613, 5260.178240499553, 5260.433206988063, 5260.687544643583, 5260.941255703583, 5261.210525337493, 5261.47908965853, 5261.746950200655, 5262.014108509731, 5262.280566138233, 5262.546324651859, 5262.811385620276, 5263.075750630341, 5263.339421272875, 5263.602399150607, 5263.864685875517, 5264.126283067522, 5264.387192358439, 5264.647415386699, 5264.9069538026315, 5265.165809261857, 5265.423983433222, 5265.681477993503, 5265.938294624769, 5266.194435023634, 5266.4499008933235, 5266.704693942355, 5266.95881589379, 5267.212268474658, 5267.465053423888, 5267.7171724883465, 5267.968627420187, 5268.219419983465, 5268.469551948848, 5268.719025097584, 5268.967841216208, 5269.216002101836, 5269.463509558196, 5269.710365396948, 5269.95657143901, 5270.202129513235, 5270.447041453766, 5270.691309106645, 5270.934934323205, 5271.1779189640365, 5271.420264893695, 5271.661973988639, 5271.90304813194, 5272.14620533551, 5272.388716660605, 5272.630583696726, 5272.8718080426315, 5273.112391303691, 5273.352335089242, 5273.591641015234, 5273.830310702905, 5274.068345776139, 5274.305747865431, 5274.5425186052435, 5274.778659632684, 5275.014172591472, 5275.249059126648, 5275.483320885898, 5275.716959520877, 5275.94997668456, 5276.1823740338905, 5276.414153227132, 5276.64531592123, 5276.875863779737, 5277.105798462242, 5277.335121633622, 5277.563834954788, 5277.791940090616, 5278.019438703338, 5278.246332455187, 5278.472623007074, 5278.698312022553, 5278.923401158568, 5279.147892073382, 5279.371786421295, 5279.59508585925, 5279.817792034933, 5280.039906597352, 5280.261431192871, 5280.482367459921, 5280.702717038253, 5280.922481561006, 5281.141662656032, 5281.36026194986, 5281.578281059759, 5281.79572160168, 5282.012585183638, 5282.228873407035, 5282.444587870629, 5282.659730161278, 5282.874301864516, 5283.088304555297, 5283.3017398019665, 5283.514609166255, 5283.726914199315, 5283.938656447009, 5284.149837444621, 5284.3604587168575, 5284.570521784454, 5284.780028153604, 5284.988979321241, 5285.197376777688, 5285.405221997401]
testPathY2 = [9225.351339797398, 9225.734252453303, 9226.117588059195, 9226.501343678718, 9226.88551635661, 9227.27010313681, 9227.679765297711, 9228.089890195168, 9228.500475096149, 9228.91151727629, 9229.323014023052, 9229.734962634924, 9230.147360419856, 9230.560204696829, 9230.97349279507, 9231.387222054838, 9231.801389826636, 9232.215993472004, 9232.631030362722, 9233.046497881607, 9233.46239342014, 9233.878714381628, 9234.2954581812, 9234.712622241856, 9235.13020399843, 9235.548200895992, 9235.966610390647, 9236.385429947963, 9236.80465704374, 9237.224289164824, 9237.6443238083, 9238.064758482284, 9238.485590703565, 9238.90681800075, 9239.328437911907, 9239.750447986133, 9240.172845781195, 9240.595628867468, 9241.018794823209, 9241.442341238499, 9241.866265712868, 9242.2905658561, 9242.715239289006, 9243.140283641067, 9243.565696552798, 9243.991475674171, 9244.417618666977, 9244.844123200104, 9245.270986955044, 9245.703042286512, 9246.135460483834, 9246.568239387698, 9247.001376851395, 9247.434870732946, 9247.868718903772, 9248.30291924081, 9248.737469633603, 9249.17236797879, 9249.60761218483, 9250.04320016885, 9250.479129857433, 9250.91539918741, 9251.352006106645, 9251.788948571664, 9252.226224550825, 9252.663832021934, 9253.101768973043, 9253.540033403244, 9253.978623322651, 9254.41753675006, 9254.856771718438, 9255.296326268644, 9255.73619845414, 9256.176386338637, 9256.616887997661, 9257.057701516991, 9257.498824995007, 9257.940256540342, 9258.381994273443, 9258.824036326581, 9259.266380843063, 9259.709025978798, 9260.15196989916, 9260.59521078449, 9261.038746824588, 9261.482576221077, 9261.926697189761, 9262.371107955905, 9262.815806758172, 9263.260791847826, 9263.706061487175, 9264.151613951917, 9264.597447527996, 9265.043560516333, 9265.48995122809, 9265.936617988613, 9266.383559133497, 9266.8307730141, 9267.278257992017, 9267.726012440671, 9268.174034749245, 9268.622323316376, 9269.070876554892, 9269.519692891798, 9269.968770764352, 9270.418108624779, 9270.867704935556, 9271.317558175702, 9271.767666833699]
testPathX = [5227.734156721738, 5228.041407657399, 5228.348788182396, 5228.656291933433, 5228.963912555148, 5229.271643693503, 5229.579479003716, 5229.887412139682, 5230.195436761907, 5230.5035465348665, 5230.81173512568, 5231.11999620411, 5231.428323443889, 5231.736710521392, 5232.0451511143165, 5232.353638903007, 5232.662167570449, 5232.970730800956, 5233.2793222801565, 5233.587935692362, 5233.896564727173, 5234.205203071544, 5234.513844415074, 5234.825430142249, 5235.137005349887, 5235.448562894486, 5235.760095633868, 5236.071596420568, 5236.383058109761, 5236.694473553981, 5237.005835600471, 5237.317137099118, 5237.628370891876, 5237.939529823344, 5238.250606728862, 5238.561594443774, 5238.872485799453, 5239.183273621985, 5239.493950732164, 5239.804509948142, 5240.114944078813, 5240.425245931749, 5240.735408306585, 5241.045423996349, 5241.3552857900995, 5241.664986464992, 5241.974518796861, 5242.283875550964, 5242.590335094706, 5242.896603221205, 5243.20266812028, 5243.508517969847, 5243.8141409306345, 5244.119525150142, 5244.4246587533935, 5244.729529852186, 5245.0341265358375, 5245.338436875151, 5245.642448922419, 5245.946150706129, 5246.249530232289, 5246.552575484424, 5246.8552744249055, 5247.157614988331, 5247.459585084176, 5247.761172596787, 5248.062365384067, 5248.3631512748225, 5248.663518070093, 5248.963453541825, 5249.262945432873, 5249.5627291389455, 5249.862049162825, 5250.160899954481, 5250.4592759559455, 5250.757171592063, 5251.0545812757755, 5251.351499405478, 5251.647920366342, 5251.943838527672, 5252.239248246869, 5252.534143866787, 5252.828519717061, 5253.122370108807, 5253.415689343888, 5253.708471706975, 5254.000711466869, 5254.292402880473, 5254.583540190139, 5254.874117618388, 5255.162556410675, 5255.450425558409, 5255.737714938764, 5256.024414395859, 5256.310513747361, 5256.596002779204, 5256.880871248226, 5257.165108880853, 5257.448705371771, 5257.731650383932, 5258.0139335525155, 5258.295544474354, 5258.57647271983, 5258.856707824948, 5259.136239291325, 5259.415056588846, 5259.693149154334, 5259.970506390229, 5260.247117664593, 5260.522972309781]
testPathY = [9181.576167455129, 9181.958978093626, 9182.34168468307, 9182.724292264751, 9183.106805886267, 9183.489230599156, 9183.871571462834, 9184.253833541447, 9184.636021902295, 9185.01814161898, 9185.400197769046, 9185.782195433188, 9186.164139696044, 9186.546035646186, 9186.92788837456, 9187.309702974468, 9187.691484541578, 9188.073238174713, 9188.454968974269, 9188.836682039071, 9189.21838247424, 9189.600075380176, 9189.981765862007, 9190.367104739631, 9190.752452123697, 9191.137813787615, 9191.52319550716, 9191.908603053378, 9192.294042197314, 9192.679518707651, 9193.065038349918, 9193.450606887282, 9193.836230080542, 9194.221913683408, 9194.60766344959, 9194.993485124913, 9195.379384452051, 9195.765367167382, 9196.151439002542, 9196.537605682082, 9196.923872924252, 9197.310246440986, 9197.696731934773, 9198.083335103367, 9198.470061634278, 9198.856917206349, 9199.243907491327, 9199.631038149142, 9200.014914683234, 9200.398943956578, 9200.783135221918, 9201.167497710723, 9201.552040636332, 9201.936773190022, 9202.321704542579, 9202.706843841146, 9203.09220020765, 9203.47778274037, 9203.8636005108, 9204.249662561268, 9204.635977908096, 9205.022555536874, 9205.409404402459, 9205.796533426601, 9206.18395149875, 9206.571667476033, 9206.959690176975, 9207.348028385413, 9207.736690848153, 9208.125686271806, 9208.51502332358, 9208.90568380477, 9209.296699677749, 9209.688074305212, 9210.079811033314, 9210.471913186138, 9210.864384072007, 9211.257226975606, 9211.650445164272, 9212.044041884075, 9212.438020362168, 9212.832383803634, 9213.227135393072, 9213.622278293797, 9214.017815648638, 9214.41375057836, 9214.810086180869, 9215.206825533585, 9215.603971690281, 9216.001527684248, 9216.397336322392, 9216.793559460033, 9217.190203181084, 9217.587273523746, 9217.984776485242, 9218.382718018664, 9218.781104029815, 9219.179940380372, 9219.579232885511, 9219.97898731313, 9220.379209380682, 9220.779904759918, 9221.181079070573, 9221.582737882738, 9221.984886714485, 9222.387531032668, 9222.790676249757, 9223.194327725427, 9223.598490764185, 9224.003170614587]
testPathX1 = [5239.370326158257, 5239.670013551381, 5239.969708984579, 5240.26940967424, 5240.569112832785, 5240.868815677927]
testPathY1 = [9184.955669108223, 9185.348096832216, 9185.740518415558, 9186.132935985252, 9186.52535166909, 9186.917767592504]
centerX = [5262.1881828545975, 5262.453606072, 5262.718301051738, 5262.982262219978, 5263.245484004207, 5263.507960830591, 5263.7696871252965, 5264.0306573025855, 5264.300546043375, 5264.569621079718, 5264.83788825389, 5265.105353433295, 5265.372022518396, 5265.637901436102, 5265.902996142417, 5266.167312619788, 5266.430856882406, 5266.693634968258, 5266.955652941783, 5267.2169168978335, 5267.47743295242, 5267.737207250644, 5267.996245964057, 5268.2545552866895, 5268.512141439021, 5268.769010664009, 5269.0251692323845, 5269.280623436033, 5269.5353795893225, 5269.789444031746, 5270.042823126599, 5270.2955232583345, 5270.547550831243, 5270.798912274739, 5271.049614040718, 5271.299662598264, 5271.5490644389465, 5271.797826078134, 5272.0459540457405, 5272.29345489813, 5272.540335204892, 5272.786601559413, 5273.032260573597, 5273.2773188765395, 5273.521783117169, 5273.76565996293, 5274.008956098455, 5274.251678224249, 5274.493833063293, 5274.73542734915, 5274.976467839185, 5275.21696130002, 5275.456914522079, 5275.696334303722, 5275.93522746711, 5276.173600844983, 5276.41146128463, 5276.648815651849, 5276.883670356001, 5277.118028725737, 5277.351885216314, 5277.585234272413, 5277.818070330778, 5278.050387820218, 5278.282181156322, 5278.513444746739, 5278.74417299119, 5278.974360276168, 5279.204000981555, 5279.4330894766545, 5279.661620118868, 5279.889587256342, 5280.11698523193, 5280.343808371298, 5280.570050993499, 5280.79570740965, 5281.0207719163245, 5281.245238803483, 5281.469102347862, 5281.692356819585, 5281.916406738968, 5282.139837186496, 5282.362651330591, 5282.584852347612, 5282.806443423174, 5283.02742775347, 5283.247808541307, 5283.46758899478, 5283.686772337854, 5283.905361794491, 5284.12336060188, 5284.340772003822, 5284.5575992507265, 5284.773845599619, 5284.989514319422, 5285.204608681706, 5285.419131965977, 5285.633087460994, 5285.846478459484, 5286.059308264756, 5286.271580181437, 5286.483297524736, 5286.694463616474, 5286.905081779793, 5287.115155348414, 5287.32468766135, 5287.5336820629, 5287.742141902654, 5287.950070535492, 5288.157471321584, 5288.364347627711]
centerY = [9223.598943221137, 9223.991989018421, 9224.385525616264, 9224.779554793347, 9225.174078287368, 9225.569097798192, 9225.964614990231, 9226.360631496365, 9226.771928011545, 9227.183757329125, 9227.596113355734, 9228.008990035836, 9228.422381353292, 9228.836281329006, 9229.250684022496, 9229.665583528744, 9230.080973982922, 9230.496849554882, 9230.913204453085, 9231.33003292146, 9231.747329239395, 9232.165087723322, 9232.58330272671, 9233.001968635339, 9233.421079873602, 9233.84063089742, 9234.26061620054, 9234.681030308227, 9235.10186778279, 9235.523123218063, 9235.944791241762, 9236.366866516279, 9236.789343736316, 9237.212217628106, 9237.635482951759, 9238.059134498124, 9238.483167091936, 9238.907575587878, 9239.332354872158, 9239.757499861713, 9240.183005505005, 9240.608866780447, 9241.035078695606, 9241.461636288785, 9241.888534627446, 9242.315768808206, 9242.74333395685, 9243.171225227523, 9243.599437801959, 9244.027966889473, 9244.456807728535, 9244.885955583628, 9245.315405746815, 9245.74515353537, 9246.17519429574, 9246.605523397211, 9247.036136235876, 9247.467028234614, 9247.894553153004, 9248.32235035397, 9248.750422118192, 9249.178770706641, 9249.607398362172, 9250.036307307933, 9250.465499749736, 9250.894977869752, 9251.324743835174, 9251.754799788769, 9252.185147855176, 9252.615790138549, 9253.046728722546, 9253.477965667973, 9253.909503015935, 9254.341342787837, 9254.77348697986, 9255.205937568491, 9255.638696508144, 9256.071765731169, 9256.505147146278, 9256.938842640111, 9257.375604387773, 9257.812683361715, 9258.25007684073, 9258.687782117791, 9259.125796501636, 9259.564117315185, 9260.002741894757, 9260.441667592435, 9260.880891776058, 9261.320411824503, 9261.760225134769, 9262.200329115676, 9262.640721191023, 9263.081398798788, 9263.522359393499, 9263.963600439147, 9264.405119418627, 9264.846913825875, 9265.288981170588, 9265.731318975066, 9266.173924775798, 9266.616796125038, 9267.059930586855, 9267.503325739504, 9267.946979174645, 9268.390888498901, 9268.835051331509, 9269.279465305104, 9269.724128066504, 9270.169037275926, 9270.614190606984]


def main():
      #源代码demo
    print(__file__ + " start!!")

    # way points
    wx = [0.0, 10.0, 20.5, 35.0, 70.5]
    wy = [0.0, 1.0, 2.0, 3.5, 4.0]
    # obstacle lists（障碍物）
    # ob = np.array([[20.0, 10.0],
    #                [30.0, 6.0],
    #                [30.0, 8.0],
    #                [35.0, 8.0],
    #                [50.0, 3.0]
    #                ])

    tx, ty, tyaw, tc, csp = generate_target_course(testPathX2, testPathY2)
    plt.plot(testPathX3, testPathY3)
    
    # tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
    # initial state

    # car = (5273,9248,0.5)
    car = (5284.05859375,9265.29589844,-0.5)
    # plt.plot(car[0], car[1], "bo")
    # plt.show()
    # car = (5280,9250,-0.5)
    # car = (10,-3,0.7)
    # line = list(zip(tx, ty))
    line = [tx,ty]
    
    s0,c_d = frenet_coordinates_improved(car,line)
    print("s0",s0)
    print("c_d",c_d)
    c_speed = 10.0 / 3.6  # current speed [m/s]
    c_accel = 0.0  # current acceleration [m/ss]
    # c_d = 2.0  # current lateral position [m]
    # c_d_d = 
    c_d_d = 0.0  # current lateral speed [m/s]
    c_d_dd = 0  # current lateral acceleration [m/s]
    target_di = 2.0

    area = 20.0  # animation area length [m]

    # for i in range(SIM_LOOP):
    # print("Time:", i)
    print("my path planning")
    if(c_d>0):
        target_di = -target_di
    path = frenet_optimal_planning(
        car[0],car[1],csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd,None,5,target_di)
    print("calculation finished")
    # if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:
    #     print("Goal")
    #     break
    print(path.s)
    if show_animation:  # pragma: no cover
        plt.cla()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        
        # plt.plot(testPathX2, testPathY2)
        # plt.plot(centerX,centerY)
        # plt.plot(testPathX1, testPathY1)
        # plt.plot(5284.29443359,9265.87792969,"-or")
        # plt.plot(ob[:, 0], ob[:, 1], "xk")
        plt.plot(path.x[1:], path.y[1:], "-or")
        # print(path.d_d)
        print(path.x)
        print(path.y)
        plt.quiver(path.x[-1], path.y[-1],2*math.sin(path.yaw[-1]),2*math.cos(path.yaw[-1]), angles='xy', scale=1, scale_units='xy')
        plt.plot(path.x[1], path.y[1], "vc")
        plt.plot(car[0], car[1], "bo")
        # plt.xlim(path.x[1] - area, path.x[1] + area)
        # plt.ylim(path.y[1] - area, path.y[1] + area)
        plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])
        plt.grid(True)
        plt.pause(0.0001)

    print("Finish")
    if show_animation:  # pragma: no cover
        plt.grid(True)
        plt.pause(0.0001)
        plt.show()


if __name__ == '__main__':
    main()
